#include <vector>
#include <cmath>
#include <algorithm>

// Define precision type (double precision in original)
using Real = double;

class SlaveNodeProjector {
public:
    // Main projection routine
    void projectSlaveNodes(
        const std::vector<std::array<Real, 3>>& nodeCoords,    // Node coordinates (X)
        const std::vector<std::array<int, 6>>& segmentConnect, // Segment connectivity (IRECT)
        const std::vector<int>& masterSegmentRefs,            // LMSR array
        const std::vector<int>& masterNodes,                  // MSR array
        const std::vector<int>& slaveNodeList,                // NSV array
        std::vector<int>& nodeLocations,                      // ILOC array
        std::vector<int>& contactSegments,                    // IRTL array
        const std::vector<int>& segmentIndices,               // NSEG array
        int numSegments,                                      // NSEG
        int numSlaveNodes,                                    // NSN
        int numMasterNodes,                                   // NMN
        int startIndex,                                       // IBEG
        int endIndex                                          // IEND
    ) {
        for (int ii = startIndex; ii <= endIndex; ++ii) {
            int slaveNode = slaveNodeList[ii];
            if (slaveNode <= 0) continue;

            int masterIndex = nodeLocations[ii];
            int masterNode = std::abs(masterNodes[masterIndex]);
            int closestMaster = masterNode;

            // Get slave node coordinates
            const auto& slavePos = nodeCoords[slaveNode];
            
            // Calculate initial minimum distance
            Real minDistSq = calculateSquaredDistance(
                slavePos, nodeCoords[masterNode]);

            // Check all segments connected to current master node
            int segStart = segmentIndices[masterIndex];
            int segEnd = segmentIndices[masterIndex + 1] - 1;

            for (int jj = segStart; jj <= segEnd; ++jj) {
                int segId = masterSegmentRefs[jj];
                const auto& segNodes = segmentConnect[segId];

                // Determine other nodes in the segment
                auto [node1, node2, node3] = getOtherSegmentNodes(
                    masterNode, segNodes);

                // Check distances to other nodes in segment
                checkNodeDistances(
                    slavePos, nodeCoords, node1, node2, node3,
                    minDistSq, closestMaster);
            }

            // Update master node if closer node found
            if (closestMaster != masterNode) {
                updateClosestMaster(
                    masterNodes, numMasterNodes, closestMaster,
                    nodeLocations[ii]);
            }

            // Verify or find new contact segment
            verifyContactSegment(
                nodeCoords, segmentConnect, slaveNode, slavePos,
                masterNodes[masterIndex], contactSegments[ii],
                segmentIndices, masterSegmentRefs, masterIndex);
        }
    }

private:
    // Helper methods
    
    Real calculateSquaredDistance(
        const std::array<Real, 3>& pos1,
        const std::array<Real, 3>& pos2
    ) {
        Real dx = pos1[0] - pos2[0];
        Real dy = pos1[1] - pos2[1];
        Real dz = pos1[2] - pos2[2];
        return dx*dx + dy*dy + dz*dz;
    }

    std::tuple<int, int, int> getOtherSegmentNodes(
        int masterNode,
        const std::array<int, 6>& segNodes
    ) {
        if (masterNode == segNodes[0]) {
            return {segNodes[1], segNodes[2], segNodes[3]};
        } else if (masterNode == segNodes[1]) {
            return {segNodes[0], segNodes[2], segNodes[3]};
        } else if (masterNode == segNodes[2]) {
            return {segNodes[0], segNodes[1], segNodes[3]};
        } else { // segNodes[3]
            return {segNodes[0], segNodes[1], segNodes[2]};
        }
    }

    void checkNodeDistances(
        const std::array<Real, 3>& slavePos,
        const std::vector<std::array<Real, 3>>& nodeCoords,
        int node1, int node2, int node3,
        Real& minDistSq, int& closestNode
    ) {
        auto checkNode = [&](int node) {
            Real distSq = calculateSquaredDistance(slavePos, nodeCoords[node]);
            if (distSq < minDistSq) {
                minDistSq = distSq;
                closestNode = node;
            }
        };

        checkNode(node1);
        checkNode(node2);
        checkNode(node3);
    }

    void updateClosestMaster(
        const std::vector<int>& masterNodes,
        int numMasterNodes,
        int closestNode,
        int& locationIndex
    ) {
        for (int j = 0; j < numMasterNodes; ++j) {
            if (closestNode == std::abs(masterNodes[j])) {
                locationIndex = j;
                return;
            }
        }
    }

    void verifyContactSegment(
        const std::vector<std::array<Real, 3>>& nodeCoords,
        const std::vector<std::array<int, 6>>& segmentConnect,
        int slaveNode,
        const std::array<Real, 3>& slavePos,
        int masterNode,
        int& contactSegment,
        const std::vector<int>& segmentIndices,
        const std::vector<int>& masterSegmentRefs,
        int masterIndex
    ) {
        const auto& masterPos = nodeCoords[masterNode];
        std::array<Real, 3> slaveVec = {
            slavePos[0] - masterPos[0],
            slavePos[1] - masterPos[1],
            slavePos[2] - masterPos[2]
        };

        // Check if current segment is still valid
        if (contactSegment != 0 && 
            checkSegmentProjection(nodeCoords, segmentConnect[contactSegment],
                                 masterNode, slaveVec)) {
            return;
        }

        // Find new segment containing the projection
        findNewContactSegment(
            nodeCoords, segmentConnect, masterNode, slaveVec,
            segmentIndices, masterSegmentRefs, masterIndex,
            contactSegment);
    }

    bool checkSegmentProjection(
        const std::vector<std::array<Real, 3>>& nodeCoords,
        const std::array<int, 6>& segmentNodes,
        int masterNode,
        const std::array<Real, 3>& slaveVec
    ) {
        int node1, node2;
        if (masterNode == segmentNodes[0]) {
            node1 = segmentNodes[1];
            node2 = segmentNodes[3];
        } else if (masterNode == segmentNodes[1]) {
            node1 = segmentNodes[2];
            node2 = segmentNodes[0];
        } else if (masterNode == segmentNodes[2]) {
            node1 = segmentNodes[3];
            node2 = segmentNodes[1];
            if (segmentNodes[2] == segmentNodes[3]) node1 = segmentNodes[0];
        } else { // segmentNodes[3]
            node1 = segmentNodes[0];
            node2 = segmentNodes[2];
        }

        const auto& pos1 = nodeCoords[node1];
        const auto& pos2 = nodeCoords[node2];
        const auto& masterPos = nodeCoords[masterNode];

        std::array<Real, 3> vec1 = {
            pos1[0] - masterPos[0],
            pos1[1] - masterPos[1],
            pos1[2] - masterPos[2]
        };

        std::array<Real, 3> vec2 = {
            pos2[0] - masterPos[0],
            pos2[1] - masterPos[1],
            pos2[2] - masterPos[2]
        };

        // Calculate normal vector
        std::array<Real, 3> normal = {
            vec1[1] * vec2[2] - vec1[2] * vec2[1],
            vec1[2] * vec2[0] - vec1[0] * vec2[2],
            vec1[0] * vec2[1] - vec1[1] * vec2[0]
        };

        // Projection calculations
        Real normalDot = normal[0] * normal[0] + 
                        normal[1] * normal[1] + 
                        normal[2] * normal[2];
        Real alpha = (normal[0] * slaveVec[0] + 
                     normal[1] * slaveVec[1] + 
                     normal[2] * slaveVec[2]) / normalDot;

        std::array<Real, 3> tangentVec = {
            slaveVec[0] - alpha * normal[0],
            slaveVec[1] - alpha * normal[1],
            slaveVec[2] - alpha * normal[2]
        };

        Real vec1Dot = vec1[0] * vec1[0] + vec1[1] * vec1[1] + vec1[2] * vec1[2];
        Real vec2Dot = vec2[0] * vec2[0] + vec2[1] * vec2[1] + vec2[2] * vec2[2];
        Real vec1vec2Dot = vec1[0] * vec2[0] + vec1[1] * vec2[1] + vec1[2] * vec2[2];
        Real tangentVec2Dot = tangentVec[0] * vec2[0] + 
                             tangentVec[1] * vec2[1] + 
                             tangentVec[2] * vec2[2];
        Real tangentVec1Dot = tangentVec[0] * vec1[0] + 
                             tangentVec[1] * vec1[1] + 
                             tangentVec[2] * vec1[2];

        // Check if projection is within segment bounds
        if (vec1Dot * tangentVec2Dot < vec1vec2Dot * tangentVec1Dot) return false;
        if (vec2Dot * tangentVec1Dot < vec1vec2Dot * tangentVec2Dot) return false;
        return true;
    }

    void findNewContactSegment(
        const std::vector<std::array<Real, 3>>& nodeCoords,
        const std::vector<std::array<int, 6>>& segmentConnect,
        int masterNode,
        const std::array<Real, 3>& slaveVec,
        const std::vector<int>& segmentIndices,
        const std::vector<int>& masterSegmentRefs,
        int masterIndex,
        int& contactSegment
    ) {
        Real maxProjection = -1e36;
        int bestSegment = 0;

        int segStart = segmentIndices[masterIndex];
        int segEnd = segmentIndices[masterIndex + 1] - 1;

        for (int jj = segStart; jj <= segEnd; ++jj) {
            int segId = masterSegmentRefs[jj];
            const auto& segNodes = segmentConnect[segId];

            int node1, node2;
            if (masterNode == segNodes[0]) {
                node1 = segNodes[1];
                node2 = segNodes[3];
            } else if (masterNode == segNodes[1]) {
                node1 = segNodes[2];
                node2 = segNodes[0];
            } else if (masterNode == segNodes[2]) {
                node1 = segNodes[3];
                node2 = segNodes[1];
                if (segNodes[2] == segNodes[3]) node1 = segNodes[0];
            } else { // segNodes[3]
                node1 = segNodes[0];
                node2 = segNodes[2];
            }

            const auto& pos1 = nodeCoords[node1];
            const auto& pos2 = nodeCoords[node2];
            const auto& masterPos = nodeCoords[masterNode];

            std::array<Real, 3> vec1 = {
                pos1[0] - masterPos[0],
                pos1[1] - masterPos[1],
                pos1[2] - masterPos[2]
            };

            std::array<Real, 3> vec2 = {
                pos2[0] - masterPos[0],
                pos2[1] - masterPos[1],
                pos2[2] - masterPos[2]
            };

            // Calculate normal vector
            std::array<Real, 3> normal = {
                vec1[1] * vec2[2] - vec1[2] * vec2[1],
                vec1[2] * vec2[0] - vec1[0] * vec2[2],
                vec1[0] * vec2[1] - vec1[1] * vec2[0]
            };

            // Projection calculations
            Real normalDot = normal[0] * normal[0] + 
                            normal[1] * normal[1] + 
                            normal[2] * normal[2];
            Real alpha = (normal[0] * slaveVec[0] + 
                         normal[1] * slaveVec[1] + 
                         normal[2] * slaveVec[2]) / normalDot;

            std::array<Real, 3> tangentVec = {
                slaveVec[0] - alpha * normal[0],
                slaveVec[1] - alpha * normal[1],
                slaveVec[2] - alpha * normal[2]
            };

            Real vec1Dot = vec1[0] * vec1[0] + vec1[1] * vec1[1] + vec1[2] * vec1[2];
            Real vec2Dot = vec2[0] * vec2[0] + vec2[1] * vec2[1] + vec2[2] * vec2[2];
            Real vec1vec2Dot = vec1[0] * vec2[0] + vec1[1] * vec2[1] + vec1[2] * vec2[2];
            Real tangentVec2Dot = tangentVec[0] * vec2[0] + 
                                 tangentVec[1] * vec2[1] + 
                                 tangentVec[2] * vec2[2];
            Real tangentVec1Dot = tangentVec[0] * vec1[0] + 
                                 tangentVec[1] * vec1[1] + 
                                 tangentVec[2] * vec1[2];

            Real proj1 = tangentVec2Dot / std::sqrt(vec2Dot);
            Real proj2 = tangentVec1Dot / std::sqrt(vec1Dot);
            Real currentProjection = std::max(proj1, proj2);

            if (maxProjection <= currentProjection) {
                bestSegment = segId;
                maxProjection = currentProjection;
            }

            // Check if projection is within segment bounds
            if (vec1Dot * tangentVec2Dot < vec1vec2Dot * tangentVec1Dot) continue;
            if (vec2Dot * tangentVec1Dot < vec1vec2Dot * tangentVec2Dot) continue;
            
            contactSegment = segId;
            return;
        }

        // If no perfect segment found, use the best one
        contactSegment = bestSegment;
    }
};